#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import rospy
from geometry_msgs.msg import Pose2D

def interpolate_pose(start, end, duration, rate_hz):
    steps = int(duration * rate_hz)
    print(f"[INFO] 开始插值计算:")
    print(f"  起始位置: x={start[0]:.3f}, y={start[1]:.3f}, theta={start[2]:.3f}")
    print(f"  目标位置: x={end[0]:.3f}, y={end[1]:.3f}, theta={end[2]:.3f}")
    print(f"  持续时间: {duration}秒")
    print(f"  发布频率: {rate_hz}Hz")
    print(f"  总步数: {steps + 1}")
    print("-" * 50)
    
    for i in range(steps + 1):
        t = float(i) / steps
        x = start[0] + t * (end[0] - start[0])
        y = start[1] + t * (end[1] - start[1])
        theta = start[2] + t * (end[2] - start[2])
        print(f"[步骤 {i+1:2d}/{steps+1}] t={t:.3f} -> x={x:.3f}, y={y:.3f}, theta={theta:.3f}")
        yield x, y, theta

def main():
    print("=" * 60)
    print("           Pose2D 测试发布器启动")
    print("=" * 60)
    
    try:
        rospy.init_node('pose2d_test_publisher')
        print("[INFO] ROS节点 'pose2d_test_publisher' 初始化成功")
    except Exception as e:
        print(f"[ERROR] ROS节点初始化失败: {e}")
        return
    
    pub = rospy.Publisher('/object/pose2d', Pose2D, queue_size=10)
    print("[INFO] 发布器创建成功，话题: /object/pose2d")
    
    rate_hz = 10  # 10 Hz -> 0.1s interval
    rate = rospy.Rate(rate_hz)
    print(f"[INFO] 发布频率设置为: {rate_hz}Hz (间隔: {1.0/rate_hz:.1f}秒)")

    start_pose = (-0.01, -0.2, 3.0)
    end_pose = (0.0, 0.0, 0.0)
    duration = 5.0  # seconds

    print("\n[INFO] 等待2秒让发布器准备就绪...")
    rospy.sleep(2.0)
    
    print("\n[INFO] 开始发布Pose2D消息...")
    message_count = 0
    
    for x, y, theta in interpolate_pose(start_pose, end_pose, duration, rate_hz):
        if rospy.is_shutdown():
            print("\n[WARN] 收到关闭信号，停止发布")
            break
            
        msg = Pose2D(x=x, y=y, theta=theta)
        pub.publish(msg)
        message_count += 1
        
        print(f"[发布] 消息#{message_count} -> Pose2D(x={x:.3f}, y={y:.3f}, theta={theta:.3f})")
        
        try:
            rate.sleep()
        except rospy.ROSInterruptException:
            print("\n[WARN] 收到中断信号")
            break
    
    print("\n" + "=" * 60)
    print(f"[INFO] 发布完成！总共发布了 {message_count} 条消息")
    print("[INFO] Pose2D测试发布器结束")
    print("=" * 60)

if __name__ == '__main__':
    try:
        main()
    except rospy.ROSInterruptException:
        print("\n[INFO] 程序被用户中断")
    except Exception as e:
        print(f"\n[ERROR] 程序运行出错: {e}")
        import traceback
        traceback.print_exc()
    finally:
        print("[INFO] 程序退出") 